/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 * File: mecanum_ik.c
 *
 * MATLAB Coder version            : 5.5
 * C/C++ source code generated on  : 08-Jul-2024 14:59:55
 */

/* Include Files */
#include "mecanum_ik.h"

/* Function Definitions */
/*
 * Arguments    : float vx
 *                float vy
 *                float wz
 *                float *w1
 *                float *w2
 *                float *w3
 *                float *w4
 * Return Type  : void
 */
void mecanum_ik(float vx, float vy, float wz, float *w1, float *w2, float *w3,
                float *w4)
{
  static const signed char a[12] = {25, 25,  25,  25, -25, 25,
                                    25, -25, -11, 11, -11, 11};
  float wheels[4];
  int i;
  for (i = 0; i < 4; i++) {
    wheels[i] =
        ((float)a[i] * vx + (float)a[i + 4] * vy) + (float)a[i + 8] * wz;
  }
  *w1 = wheels[0] * 0.04F;
  *w2 = wheels[1] * 0.04F;
  *w3 = wheels[2] * 0.04F;
  *w4 = wheels[3] * 0.04F;
}

/*
 * File trailer for mecanum_ik.c
 *
 * [EOF]
 */
